{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center><img src=\"../image/CLBLOGO.jpg\" alt=\"创乐博\" style=\"width: 300px;\"/></center>\n",
    "\n",
    "# 9.机器人二维码识别控制运动\n",
    "\n",
    "@－－－－湖南创乐博智能科技有限公司－－－－<br>\n",
    "@  文件名：9.机器人二维码识别控制运动.ipynb <br>\n",
    "@  版本：V2.0 <br>\n",
    "@  author: zhulin<br>\n",
    "@  说明：机器人二维码识别控制运动<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 为了可以打开摄像头，这里关闭摄像头所有进程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入必要的库\n",
    "import cv2\n",
    "import numpy as np\n",
    "from pyzbar.pyzbar import decode\n",
    "from PIL import Image, ImageDraw, ImageFont\n",
    "from LOBOROBOT import LOBOROBOT # 载入机器人库\n",
    "import  RPi.GPIO as GPIO \n",
    "import sys \n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "if sys.getdefaultencoding() != 'utf-8':\n",
    "    reload(sys)\n",
    "    sys.setdefaultencoding('utf-8')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "clbrobot = LOBOROBOT()  # 实例化机器人对象\n",
    "# Configure min and max servo pulse lengths\n",
    "servo_min = 150  # Min pulse length out of 4096\n",
    "servo_max = 600  # Max pulse length out of 4096"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 智能机器人运动状态\n",
    "enStop = 0\n",
    "enRun = 1\n",
    "enBack = 2\n",
    "enLeft = 3\n",
    "enRight = 4\n",
    "car_State = 0\n",
    "symbolPos = []\n",
    "\n",
    "last_qr_data = 'no qrcode'\n",
    "qr_data = ' '"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 二维码识别框绘制"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_rect(img, pos, color, width):\n",
    "    cv2.line(img, pos[0], pos[1], color, width)\n",
    "    cv2.line(img, pos[0], pos[3], color, width)\n",
    "    cv2.line(img, pos[2], pos[1], color, width)\n",
    "    cv2.line(img, pos[2], pos[3], color, width)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def qr_scan_decode(img):\n",
    "    global qr_data\n",
    "    global last_qr_data\n",
    "    \n",
    "    for barcode in decode(img):\n",
    "        (x, y, w, h) = barcode.rect\n",
    "        cv2.rectangle(img, (x, y), (x + w, y + h), (255, 255, 0), 2)\n",
    "        barcodeData = barcode.data.decode(\"utf-8\")\n",
    "        barcodeType = barcode.type\n",
    "        if not len(barcodeData) == 0:\n",
    "            qr_data = str(barcodeData)\n",
    "            print(qr_data)\n",
    "        else:\n",
    "            qr_data = 'no qrcode'\n",
    "            \n",
    "    if last_qr_data != qr_data:       \n",
    "        command_resolve(qr_data)\n",
    "    last_qr_data = qr_data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def command_resolve(command):\n",
    "    last_car_state = 0\n",
    "    if command.find(\"Run\",0,len(command)) != -1 :\n",
    "        command.zfill(len(command))\n",
    "        car_state = 1\n",
    "    elif command.find(\"Back\",0,len(command)) != -1 :\n",
    "        command.zfill(len(command))\n",
    "        car_state = 2\n",
    "    elif command.find(\"Left\",0,len(command)) != -1 :\n",
    "        command.zfill(len(command))\n",
    "        car_state = 3\n",
    "    elif command.find(\"Right\",0,len(command)) != -1:\n",
    "        command.zfill(len(command))\n",
    "        car_state = 4\n",
    "    elif command.find(\"Stop\",0,len(command)) != -1:            \n",
    "        command.zfill(len(command))\n",
    "        car_state = 5\n",
    "    else:\n",
    "        car_state = 5\n",
    "        command.zfill(len(command))\n",
    "        \n",
    "    if last_car_state != car_state :\n",
    "        car_control(car_state)\n",
    "    last_car_state = car_state"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def car_control(car_state):\n",
    "    print(car_state)\n",
    "    if car_state == enRun:\n",
    "        content='机器人前进'\n",
    "        clbrobot.t_up(30,1)         # 前进\n",
    "    elif car_state == enBack :\n",
    "        content='机器人后退'\n",
    "        clbrobot.t_down(30,1)      # 后退\n",
    "    elif car_state == enLeft :\n",
    "        content='机器人左转'      \n",
    "        clbrobot.turnLeft(30,1)    # 左转\n",
    "    elif car_state == enRight :\n",
    "        content='机器人右转'\n",
    "        clbrobot.turnRight(30,1)   # 右转\n",
    "    elif car_state == enStop:\n",
    "        content='机器人停止'\n",
    "        clbrobot.t_stop(0)        # 停止\n",
    "    else:\n",
    "        content='机器人停止'\n",
    "        clbrobot.t_stop(0)       # 停止\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 线程函数操作库\n",
    "import threading # 线程\n",
    "import ctypes\n",
    "import inspect\n",
    "\n",
    "# 线程结束代码\n",
    "def _async_raise(tid, exctype):\n",
    "    tid = ctypes.c_long(tid)\n",
    "    if not inspect.isclass(exctype):\n",
    "        exctype = type(exctype)\n",
    "    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))\n",
    "    if res == 0:\n",
    "        raise ValueError(\"invalid thread id\")\n",
    "    elif res != 1:\n",
    "        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)\n",
    "        raise SystemError(\"PyThreadState_SetAsyncExc failed\")\n",
    "        \n",
    "def stop_thread(thread):\n",
    "    _async_raise(thread.ident, SystemExit)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bb59fed9008d466991c099e82f48096e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Image(value=b'', format='jpeg', height='240', width='320')"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "def bgr8_to_jpeg(value, quality=75):\n",
    "    return bytes(cv2.imencode('.jpg', value)[1])\n",
    "    \n",
    "frame_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "display(frame_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Video_display():\n",
    "    # 设置Camera\n",
    "    cap = cv2.VideoCapture(0,cv2.CAP_V4L2)\n",
    "    \n",
    "    if cap.isOpened()==0:\n",
    "        print(\"camera iserror\")\n",
    "        \n",
    "    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)\n",
    "    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)\n",
    "\n",
    "    while(True):\n",
    "        grabbed, frame = cap.read()\n",
    "        frame=cv2.flip(frame, 0) \n",
    "        qr_scan_decode(frame)\n",
    "        frame_img.value = bgr8_to_jpeg(frame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "5\n",
      "Run\n",
      "1\n",
      "Run\n",
      "Run\n",
      "Run\n",
      "Run\n",
      "Run\n",
      "Stop\n",
      "5\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n",
      "Stop\n"
     ]
    }
   ],
   "source": [
    "# 开始线程\n",
    "t = threading.Thread(target=Video_display)\n",
    "t.setDaemon(True)\n",
    "t.start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 结束线程\n",
    "stop_thread(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'cap' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[19], line 3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;66;03m#GPIO.release()\u001b[39;00m\n\u001b[1;32m      2\u001b[0m clbrobot\u001b[38;5;241m.\u001b[39mt_stop(\u001b[38;5;241m0\u001b[39m)\n\u001b[0;32m----> 3\u001b[0m \u001b[43mcap\u001b[49m\u001b[38;5;241m.\u001b[39mrelease()\n",
      "\u001b[0;31mNameError\u001b[0m: name 'cap' is not defined"
     ]
    }
   ],
   "source": [
    "clbrobot.t_stop(0)\n",
    "cap.release()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
